#include "GameMath.h"

namespace ds {

Vec2 const Vec2::ZERO  = Vec2(0.0f,0.0f);
Vec2 const Vec2::RIGHT = Vec2(1.0f,0.0f);
Vec2 const Vec2::LEFT  = Vec2(0.0f,-1.0f);
Vec2 const Vec2::UP    = Vec2(0.0f,1.0f);
Vec2 const Vec2::DOWN  = Vec2(0.0f,-1.0f);

Vec2 operator + (const Vec2& v1,const Vec2& v2) {
	return Vec2(v1.x + v2.x,v1.y + v2.y);
}
Vec2 operator + (const Vec2 &v, const float s) {
	return Vec2(v.x + s,v.y + s);
}
Vec2 operator + (const float s, const Vec2 &v) {
	return Vec2(v.x + s,v.y + s);
}

Vec2 operator - (const Vec2& v1,const Vec2& v2) {
	return Vec2(v1.x - v2.x,v1.y - v2.y);
}
Vec2 operator - (const Vec2 &v, const float s) {
	return Vec2(v.x -s,v.y-s);
}
Vec2 operator - (const float s, const Vec2 &v) {
	return Vec2(v.x -s,v.y-s);
}
bool operator == (const Vec2 &u, const Vec2 &v) {
	return (u.x == v.x && u.y == v.y);
}
bool operator != (const Vec2 &u, const Vec2 &v) {
	return (u.x != v.x || u.y != v.y);
}
Vec3 const Vec3::ZERO   = Vec3(0.0f);
};

// ---------------------------------------------
//
// ---------------------------------------------

namespace ds {

namespace math {

void clamp(int* value,int min,int max) {
	if ( *value > max ) {
		*value = max;
	}
	if ( *value < min ) {
		*value = min;
	}
}
// ---------------------------------------------
// Returns a smooth value between 0.0 and 1.0
// ---------------------------------------------
float smoothstep(float x) {
	return x * x * ( 3.0f - 2.0f * x);
}

float cubicPulse( float c, float w, float x ) {
	x = fabsf(x - c);
	if( x>w ) return 0.0f;
	x /= w;
	return 1.0f - x*x*(3.0f-2.0f*x);
}

float impulse( float k, float x ) {
	const float h = k*x;
	return h*expf(1.0f-h);
}

float almostIdentity( float x, float m, float n ) {
	if( x>m ) return x;
	const float a = 2.0f*n - m;
	const float b = 2.0f*m - 3.0f*n;
	const float t = x/m;
	return (a*t + b)*t*t + n;
}

Vec2 getRadialVelocity(float angle,float velocity) {
	float vx = (float)cos(DEGTORAD(angle))*velocity;
	float vy = (float)sin(DEGTORAD(angle))*velocity;
	return Vec2(vx,vy);
}

float getAngle(float x1,float y1,float x2,float y2) {
	double x = x2 - x1;
	double y = y2 - y1;	
	double ang = atan2(y,x);
	return (float)ang;
}

float getAngle(const Vec2& v1,const Vec2& v2) {	
	if ( v1 != v2 ) {
		Vec2 vn1 = v1.unit();
		Vec2 vn2 = v2.unit();
		float dot = vn1.Dot(vn2);		
		if ( dot < -1.0f ) {
			dot = -1.0f;
		}
		if ( dot > 1.0f ) {
			dot = 1.0f;
		}
		float tmp = acos(dot);
		float cross = (vn1.x * vn2.y) - (vn2.x * vn1.y);
		if ( cross < 0.0f ) {
			tmp = 2.0f * D3DX_PI - tmp;
		}
		return tmp;
		/*
		Vec2 vc = vn1.Cross(vn2);
		float mag = vc.Length();

		float ab = atan2(mag,dot);
		if ( dot < 0.0f ) {
			ab = 2.0f * D3DX_PI - ab;
		}
		return ab;
		*/
	}
	else {
		return 0.0f;
	}
}

float getTargetAngle(const Vec2& v1,const Vec2& v2) {	
	Vec2 diff = v1 - v2;
	return getAngle(Vec2::RIGHT,diff) + D3DX_PI;
}

Vec2 getDistantPosition(const Vec2& initialPosition,float angle,float radius) {
	return getDistantPosition(initialPosition.x,initialPosition.y,angle,radius);
}

Vec2 getDistantPosition(float x,float y,float angle,float radius) {
	ds::Vec2 ret;
	ret.x = x + cos(angle) * radius;
	ret.y = y + sin(angle) * radius;
	return ret;
}

}

}
